#include "Header.h"

#define pi 3.1415926

double servo_angle[4] = {0, 0, 0, 0}; // 存放角度
double error0[7], error1[7], error_all[7];
double kp = 0.04, ki = 0.001, kd;

double L0 = 1100;
double L1 = 1100;
double L2 = 940;
double L3 = 1700;

int abs(int a)
{
    return a > 0 ? a : -a;
}

/*
函数描述：对指定坐标指定α角进行姿态解算
函数参数：
    x: 目标坐标x轴坐标（单位mm）
    y: 目标坐标y轴坐标（单位mm）
    z: 目标坐标z轴坐标（单位mm）
函数返回值：
    函数返回0时，表示有解；函数返回值不为0时，表示无解
*/
int kinematics_analysis(double x, double y, double z, double Alpha)
{
    float theta0, theta1, theta2, theta3; // 存储D0-D3四个舵机的旋转角度
    float l0, l1, l2, l3;                 // 设置变量存储四个关节长度
    float aaa, bbb, ccc, zf_flag;         // 中间变量，往下看

    // 将得到的坐标放大10倍
    x = x * 10;
    y = y * 10;
    z = z * 10;

    // 将四个关节长度分别赋值，方便使用
    l0 = L0;
    l1 = L1;
    l2 = L2;
    l3 = L3;

    /*****对应2、的第二步*************************************************************/
    // 如果平面坐标系x的坐标为0，则视觉机械臂中的theta0角置0
    // theta0角为平面坐标系x与y的夹角
    // 否则则得到xy之间的角度（弧度制）
    if (x == 0)
    {
        theta0 = 0.0;
    }
    else
    {
        theta0 = atan(x / y) * 180.0 / pi;
    }

    /*****对应2、的第三步*************************************************************/
    // 算平方根
    y = sqrt(x * x + y * y);                   // 公式（4-3）    sqrt函数是算平方根
    y = y - l3 * cos(Alpha * pi / 180.0);      // 公式（4-5）
    z = z - l0 - l3 * sin(Alpha * pi / 180.0); // 公式（4-6）

    //
    if (z < -l0)
    {
        return 1;
    }
    //
    if (sqrt(y * y + z * z) > (l1 + l2))
    {
        return 2;
    }
    //
    ccc = acos(y / sqrt(y * y + z * z));                                        // ccc为β1的角度，公式（4-8），图4.6
    bbb = (y * y + z * z + l1 * l1 - l2 * l2) / (2 * l1 * sqrt(y * y + z * z)); // 公式（4-9）bbb是cosβ2
    // 由于cos只能在-1和1之间，所以有以下判断
    if (bbb > 1 || bbb < -1)
    {
        return 5;
    }
    //
    if (z < 0)
    {
        zf_flag = -1;
    }
    else
    {
        zf_flag = 1;
    }
    //
    theta1 = ccc * zf_flag + acos(bbb); // 公式（4-10）
    theta1 = theta1 * 180.0 / pi;
    //
    if (theta1 > 180.0 || theta1 < 0.0)
    {
        return 6;
    }
    //

    /******对应2、的第四步************************************************************/
    aaa = -(y * y + z * z - l1 * l1 - l2 * l2) / (2 * l1 * l2); // 公式（4-11），aaa为cosβ3
    if (aaa > 1 || aaa < -1)
    {
        return 3;
    }
    theta2 = acos(aaa);
    theta2 = 180.0 - theta2 * 180.0 / pi;
    //
    if (theta2 > 135.0 || theta2 < -135.0)
    {
        return 4;
    }
    //

    /******对应2、的第五步************************************************************/
    theta3 = Alpha - theta1 + theta2; // 公式（4-13）
    if (theta3 > 90.0 || theta3 < -90.0)
    {
        return 7;
    }

    // 到此，D0-D3四个舵机的角度已经解出，分别给相应控制舵机的定时器通道赋值即可

    servo_angle[0] = theta0;
    servo_angle[1] = theta1 - 90;
    servo_angle[2] = theta2 - 90;
    servo_angle[3] = -theta3;

    return 0; // 返回0之后舵机才会行动
}

/*
函数描述：控制所有舵机达到目标角度
函数参数：无
函数返回值：无
*/
void Control_ToTargetAngle(void)
{
    while(1)
    {
        uint8_t flag = 1;
        for (int i = 1; i <= 6; i++)
        {
            if((int8_t)angle_target[i] == (int8_t)angle_now[i])
            {
                continue;
            }
            error1[i] = error0[i];
            error0[i] = angle_target[i] - angle_now[i];
            error_all[i] += error0[i];
            angle_now[i] += error0[i] * kp + error_all[i] * ki + (error0[i] - error1[i]) * kd;
            Servo_SetAngle(i, angle_now[i]);
            Delay_ms(10);
        }
        for (int i = 1; i <= 6; i++)
        {
            if((int8_t)angle_target[i] != (int8_t)angle_now[i])
            {
                flag = 0;
            }
        }
        if(flag)
        {
            break;
        }
    }
}

/*
函数描述：控制机械臂以最佳姿态（Servo4转角最大的情况）运动到目标坐标
函数参数：
    x: 目标坐标x轴坐标（单位mm）
    y: 目标坐标y轴坐标（单位mm）
    z: 目标坐标z轴坐标（单位mm）
函数返回值：无
*/
void Control_Poster(double x, double y, double z)
{
    for (int i = -135; i < 0; i++)
    {
        if (kinematics_analysis(x, y, z, i) == 0)
        {
            angle_target[1] = servo_angle[0];
            angle_target[2] = servo_angle[1];
            angle_target[3] = servo_angle[2];
            angle_target[4] = servo_angle[3];
            break;
        }
    }
    Control_ToTargetAngle();
}


/*
函数描述：控制夹子执行夹取操作
*/
void Control_GriperGrip(void)
{
    angle_target[6] = 0;
    Control_ToTargetAngle();
}

void Control_GriperRelease(void)
{
    angle_target[6] = -60;
    Control_ToTargetAngle();
}

/*
函数描述：进行舵机复位
函数参数：无
函数返回值：无
*/
void Control_Reset(void)
{
    angle_target[1] = 0;
    angle_target[2] = 0;
    angle_target[3] = 0;
    angle_target[4] = 0;
    angle_target[5] = 0;
    angle_target[6] = -60;
    Control_ToTargetAngle();
}
